import math

from bins.tool import interpolate_trajectory_with_heading, normalize_angle
from testModel.draw import *
from src.MpcControl.MPC_lat_lon import MPCController
from src.common.processedMatrix import ProcessedMatrix
from src.common.processedmeasurement import ProcessedMeasurement
import numpy as np
import time

trun_num = 1

def calculate_point(state, h, v):
    global dt
    h = - h * 180 / np.pi
    dt = dt_ / trun_num
    beta = -math.atan(math.tan(h * math.pi / 180) / 2) * 180 / math.pi
    # print(beta)
    x = state[0] + v * dt * math.cos((state[2] + beta) * math.pi / 180)
    y = state[1] + (v * dt) * math.sin((state[2] + beta) * math.pi / 180)
    # self.x = self.x + self.v * self.dt * math.cos((self.ho + beta)*math.pi/180)
    # self.y = self.y + self.v * self.dt * math.sin((self.ho + beta) * math.pi / 180)
    # self.ho = self.ho + self.v * self.dt * 180 / (L * math.pi) * 2 * math.sin(beta*math.pi/180)
    # R = math.sqrt(L ** 2 * (1 / math.tan(delta * math.pi / 180) ** 2 + 1 / 4))
    if h != 0:
        L1 = math.sqrt(1.18 ** 2 * (1 / math.tan(h * math.pi / 180) ** 2 + 1 / 4))
        # L1 = L/math.tan(delta*math.pi/180)
        if h > 0:
            state[2] = state[2] - (v * dt) * 180 / (math.pi * L1)
        elif h < 0:
            state[2] = state[2] + (v * dt) * 180 / (math.pi * L1)
    state[0] = x
    state[1] = y
    if state[2] > 360:
        state[2] = state[2] - 360
    elif state[2] < 0:
        state[2] = state[2] + 360
    state[2] = normalize_angle(state[2])
    return state


dtype = np.dtype([
            ('x', np.float32),
            ('y', np.float32),
            ('h', np.float32),
            ('v', np.float32),
            ('t', np.float32)
        ])
# 读取轨迹
file_path = '../trajectory/double_curve1_v5_path_velsmooth_v5.txt'
trajectories = np.loadtxt(file_path, delimiter=' ', dtype=dtype)
trajectories_unique = np.unique(trajectories, axis=0)
# 提取轨迹中的变量（假设列顺序是 x, y, v, h）
x = trajectories_unique['x']
y = trajectories_unique['y']
v = trajectories_unique['v']
h = trajectories_unique['h']  # 航向角

t_, x_new, y_new, v_new, h_new = interpolate_trajectory_with_heading(x, y, v, h, dt=0.1)
# 重新组合插值后的轨迹
trajectories_processed = np.zeros(len(x_new), dtype=dtype)
trajectories_processed['x'] = x_new
trajectories_processed['y'] = y_new
trajectories_processed['v'] = v_new
trajectories_processed['h'] = h_new
trajectories_processed['t'] = t_


tk = ProcessedMeasurement()
tk.init_traj(trajectories_processed)
current_state = [-1161.3463034149213, -12020.560898871827, 3.1415858854223577, 0, 0]
# 更新车辆当前位置
N = 25
tk.predicted_step = N
k = 0
v = 0

result_x = []
result_y = []
# drawTraj(tk.traj_point)
N_ = 0
dt_ = 0.1
begin_time = time.time()
# while N_ > 0:
mpc = MPCController()
mpc.init_mpc(N=N, dt=dt_)
a = ProcessedMatrix()

while abs(len(tk.traj_point) - tk.nearest_index) > 4:
# while N_ < 2000:

    tk.update_state(current_state)
    tk.calculate_ref_point()
    list_ = tk.get_predict_traj()
    list_v = list_['v']
    xk_ = tk.calculate_current_state

    xk_z = np.array([[float(xk_['y'][0])], [float(xk_['h'][0])], [k]])
    uk = np.array([[k]])

    mpc.update_state(xk_, list_)
    steer, a = mpc.update_control()
    print("steer = ", steer)
    # a.init_process(xk_z, uk, N, v_ref=list(list_v))
    # a.calculate_x_ref(list_)
    # a.calculate_A(list(list_v))
    #
    # a.calculate_H_g()
    # res = a.test_two()
    res = steer
    if res is None:
        pass
    else:
        k = float(res)
    # k = float(a.test_two())
    # res = a.test_qp()
    # k = float(res[0][0])
    if v + a * dt_ < 0:
        v = 0
    elif v + a * dt_ > 23:
        v = 22
    else:
        v = v + a * dt_
    # current_state = calculate_point(current_state, - k, 0.5)
    # i -= 1
    # result_x.append(current_state[0])
    # result_y.append(current_state[1])
    i = trun_num
    print("a=", a, ", v=", v)
    v = 3
    while i > 0:
        current_state = calculate_point(current_state, k, v)
        i -= 1
        result_x.append(current_state[0])
        result_y.append(current_state[1])
    current_state[3] = v
    N_ += 1

end_time = time.time()
draw_diff(tk.traj_point, [result_x, result_y])
print("all time: ", end_time - begin_time)
print("N: ", N_)
print("average time: ", (end_time - begin_time) / N_)

#
# xk = np.array([[0], [0], [0]])
# uk = np.array([[0]])
# begin = time.time()
#
# # 计算车辆下一步参考点
#
#
# print(list_['y'])
#
#
# end = time.time()
# print(end - begin)
#
# res = a.test_qp()
# end2 = time.time()
# print(end2 - end)
#
# x_dis = a.calculate_res_x(res)
# y_dis = []
# y = []
# for i in range(N):
#     y.append(float(a.x_ref[i * 3]))
#     y_dis.append(float(x_dis[i * 3]))
# drawY_diff(y, y_dis)
# print(y_dis)
# # print("Ad = ")
# # print(a.Ad)
# # print("Bd = ")
# # print(a.Bd)
# # print("H = ")
# # print(a.H)
# # print("g = ")
# # print(a.g)
#
#
#

